# -*-coding:utf-8-*-
__author__ = 'wss'

import networkx as nx
import numpy as np
from math import *


def heuristic(a, b):
    """
    返回空间距离
    :param a:
    :param b:
    :return:
    """
    (x1, y1) = a
    (x2, y2) = b
    abs_x = abs(x1 - x2)
    abs_y = abs(y1 - y2)
    return min((abs_x,abs_y))*sqrt(2)+max((abs_x,abs_y))-min((abs_x,abs_y))

def get_astar_path_weight(diagram,start, goal):
    """
    根据astar算法得到的最短路径，求得最短路径权重
    :param diagram:
    :param start:
    :param goal:
    :return:
    """
    path = nx.astar_path(diagram, start, goal)
    path_weight = 0
    for idx in range(len(path) - 1):
        path_weight += diagram[path[idx]][path[idx + 1]]['weight']
    return path_weight

def cross(p1,p2,p3):
    """
    跨立实验
    :param p1:
    :param p2:
    :param p3:
    :return:
    """
    x1 = p2[0] - p1[0]
    y1 = p2[1] - p1[1]
    x2 = p3[0] - p1[0]
    y2 = p3[1] - p1[1]
    return x1*y2-x2*y1

def is_segment_intersect(p1,p2,p3,p4):
    """
    判断两条线段是否相交,p1p2、p3p4分别为两条线段
    :param p1:
    :param p2:
    :param p3:
    :param p4:
    :return:
    """
    if (max(p1[0],p2[0])>=min(p3[0],p4[0]) and max(p3[0],p4[0])>=min(p1[0],p2[0])
        and max(p1[1],p2[1])>=min(p3[1],p4[1]) and max(p3[1],p4[1])>=min(p1[1],p2[1])):
        if(cross(p1,p2,p3)*cross(p1,p2,p4)<=0 and
           cross(p3,p4,p1)*cross(p3,p4,p2)<=0):
            return True
        else:
            return False
    else:
        return False

def get_center_node_index(edge_arr):
    """
    根据（开始结点，结束结点）矩阵，找到最中心点
    :param edge_arr:边矩阵
    :return:
    """
    mean_loc = np.mean(edge_arr[:, :2], axis=0)
    mean_dis = np.array([heuristic((x[0],x[1]),(mean_loc[0],mean_loc[1])) for x in edge_arr])
    mean_idx = mean_dis.argmin()
    start_node = edge_arr[mean_idx, 0]
    return start_node


def find_shortest_path(node_list, diagram,pin_num=2):
    """
    结合空间距离和networkx中的a*算法距离，采用探针算法
    :param node_list:风机点位坐标（以网格编号形式给出）
    :param diagram: 网格图
    :param pin_num:每次最大探针个数
    :return:返回最小生成树连接边信息、总权重
    """
    node_list =list(node_list)
    weight_matrix = []  # 下三角矩阵
    weight_matrix_all = []  # 全矩阵
    for i,idx_i in enumerate(node_list):
        for j,idx_j in enumerate(node_list):
            if (i < j):
                weight_matrix.append([i, j, heuristic(idx_i, idx_j)])
            weight_matrix_all.append([i, j, heuristic(idx_i, idx_j)])
    # weight_matrix = np.array(weight_matrix)
    result = []
    if (len(node_list) <= 0) or ((len(weight_matrix))< len(node_list)-1):
        return result  # 边数过少，则返回空
    edge_list = weight_matrix  # 空间距离矩阵
    edge_list.sort(key=lambda x:x[2])  # 对边按权重排序
    weight_sorted = np.array(edge_list)
    start_node = get_center_node_index(weight_sorted)
    weight_matrix_all.sort(key=lambda x:x[2])
    weight_sorted_all = np.array(weight_matrix_all)

    nodelist_explored = []  # 已探索点liest
    nodelist_explored.append(start_node)  # 选择开始探索的点,后期需选择中心点作为起始点
    nodeinfo_explored = {}
    nodeinfo_explored[start_node] = []  # 存储已连结点
    edgeinfo_explored = []  # 保存已连接的边
    nodeinfo_pin = {}
    nodeinfo_pin[start_node] = []  # 存储探针信息
    # pin_num = 2
    estar_weight_explored = []
    result_weight = 0
    while not len(nodelist_explored)==len(node_list):
        print nodelist_explored
        pin_weight = []  # 存储一次探索所有探针权重
        pin_candidate = []  # 待探索的边
        for idx in nodelist_explored:  # 对探索过的点展开探针
            tmp = weight_sorted_all[
                  (weight_sorted_all[:, 0] == idx) & [x not in nodelist_explored for x in weight_sorted_all[:, 1]], :]
            if nodeinfo_explored.has_key(idx) and len(nodeinfo_explored[idx])<2:  # 如果已连边数小于2，则产生两个探针
                # 找出开始结点为idx的，且结束结点不在已探索结点列表中的
                pin_list = list(tmp[:pin_num,1])  # 有问题：怎么排除已连接的结点
                for pin in pin_list:
                    pin_candidate.append([idx,pin,tmp[(tmp[:,0]==idx)&(tmp[:,1]==pin),2][0]])
            elif nodeinfo_explored.has_key(idx) and len(nodeinfo_explored[idx]) >= 2:  # 若已连接超过2条边，则只产生一个探针
                pin = tmp[0, 1]
                pin_candidate.append([idx,pin,tmp[(tmp[:,0]==idx)&(tmp[:,1]==pin),2][0]])
        pin_candidate.sort(key=lambda x: x[2]) # 根据权重排序
        pin_candidate_arr = np.array(pin_candidate)
        pin_candidate_arr = pin_candidate_arr[pin_candidate_arr[:,2]<=pin_candidate_arr[0,2]*2,:]  # 删除权重为最小权重两倍以上的边
        for edge in list(pin_candidate_arr):
            # print edge
            intersect_flag = False
            for linked_e in edgeinfo_explored:  # 判断edge边是否与已连边相交,edge起点所在边排除
                if is_segment_intersect(node_list[int(edge[0])],node_list[int(edge[1])],linked_e[0],linked_e[1]) \
                        and edge[0] not in [linked_e[2],linked_e[3]]:
                    intersect_flag = True
                    break
            if not intersect_flag:
                estar_weight_explored_arr = np.array(estar_weight_explored)
                if len(estar_weight_explored)>0 and \
                    len(estar_weight_explored_arr[(estar_weight_explored_arr[:,0]==edge[0])&(estar_weight_explored_arr[:,1]==edge[1]),:])>0:  # 如果已求过astar距离
                    dis_tmp = estar_weight_explored_arr[(estar_weight_explored_arr[:,0]==edge[0])&(estar_weight_explored_arr[:,1]==edge[1]),2][0]
                else:
                    dis_tmp = get_astar_path_weight(diagram, node_list[int(edge[0])], node_list[int(edge[1])])
                pin_weight.append([edge[0],edge[1],dis_tmp])
                estar_weight_explored.append([edge[0], edge[1],dis_tmp])  # 存储所有经历astar算法的边
                estar_weight_explored.append([edge[1], edge[0], dis_tmp])
        pin_weight.sort(key=lambda x: x[2])  # 根据权重排序，选择最小的权重作为新的边
        nodeinfo_explored.setdefault(pin_weight[0][0],[]).append(pin_weight[0][1])
        nodeinfo_explored.setdefault(pin_weight[0][1],[]).append(pin_weight[0][0])
        edgeinfo_explored.append([node_list[int(pin_weight[0][0])],node_list[int(pin_weight[0][1])],pin_weight[0][0],pin_weight[0][1]])  # 存储已连接边
        nodelist_explored.append(pin_weight[0][1])  # 将终点加入已探索结点
        result_weight+=pin_weight[0][2]
    # 将序号转为坐标
    result_edge = {}
    for key in nodeinfo_explored.keys():
        result_edge[node_list[int(key)]] = []
        for val in nodeinfo_explored[key]:
            result_edge[node_list[int(key)]].append(node_list[int(val)])

    return result_edge, result_weight  # 返回连接边信息及总权重